It is desirable in many applications to estimate absolute and relative 3-D position and orientation for people and objects using low-cost, small size, weight and form-factor navigation devices. In recent times, the Global Positioning System (GPS) for military and civilian applications has been the technology of choice for fulfilling this need. Unfortunately, GPS may be unavailable in several situations where the GPS signals become weak, corrupted, or non-existent. Such situations include urban canyons, indoor locations, underground locations, or areas where GPS signals are being jammed or subject to RF interference. Thus there are several applications where people/objects are operating in environments where GPS is not accessible, which may include civil and military applications such as security, intelligence, and emergency first responder activities.
A traditional technique for obtaining 3-D locations of mobile people/objects (hereinafter “nodes”) is by means of trilateration. In trilateration, the position of a mobile node can be calculated using the known positions of multiple RF reference beacons (anchors) and measurements of the distances between the mobile node and the anchors. The anchor nodes can pinpoint the mobile node by geometrically forming four or more spheres surrounding the anchor nodes which intersect at a single point that is the location of the mobile node. Unfortunately, this technique has strict infrastructure requirements, requiring at least three anchor nodes for a 2D position and four anchor nodes for a 3D position. The technique is further complicated by being heavily dependent on relative node geometry and suffers from accuracy errors due to RF propagation complexities.
Many other sensor networks are based on position measurements in such techniques as received signal strength (RSS), the angle of arrival (AoA), the time of arrival (ToA) or time difference of arrival (TDoA) of signals between nodes, including stationary anchor nodes. There are two disadvantages for such approaches: (i) anchor nodes are an infrastructure that is inconvenient in certain applications, like soldiers behind enemy lines; (ii) estimation ambiguities such as translation, orientation, and flip exist for a network without enough anchor nodes. Ambiguities can be eliminated by deploying a sufficient number of anchor nodes in a mobile sensor network, but this incurs the increased infrastructure costs of having to deploy multiple anchor nodes. Inertial navigation units (INUs), consisting of accelerometers, gyroscopes and magnetometers, may be employed to track an individual node's position and orientation over time. Highly accurate INUs are typically expensive, bulky, heavy, power-intensive, and may place limitations on node mobility. INUs with lower size, weight, power and cost are typically also much less accurate. Such systems using only inertial navigation unit (INU) measurements have a divergence problem due to the accumulation of drift error, while systems based on inter-node ranging for sensor localization suffer from flip and rotation ambiguities.
The localization problem has been studied both in the sensor-network and sensor-fusion communities. In the sensor-network community, several general surveys summarized different aspects of the technology, but only a few works have considered localization with mobile nodes. In P. Bergamo and G. Mazzini, “Localization in sensor networks with fading and mobility Personal,” The 13th IEEE International Symposium Indoor and Mobile Radio Communications, 2002, vol. 2, pp 750-754 (hereinafter “Bergamo and Mazzini”), a scheme is proposed to perform localization of mobile nodes based on the estimation of the power received by two beacons in an area of 100 m×100 m. Under the assumptions that (i) the two anchor nodes are deployed at two corners of a rectangular space, and (ii) the power can transmit across the entire network, the actual distance between the sensor and the beacons is derived and the node position can be obtained by means of triangulation. Bergamo and Mazzini also demonstrate that mobility makes localization more difficult and the errors increased with the increasing of node speed. In L. Hu and D. Evans, “Localization for mobile sensor networks,” Proc. of Int'l Conf. on Mobile Computing and Networking. Philadelphia, Pa., 2004, pp 45-57 (hereinafter “Hu and Evans”), it is argued that the mobility of the sensor network can improve both the accuracy and precision of localization by using the sequential Monte Carlo localization technique. The simulation results of Hu and Evans show that it outperforms the best known static localization schemes under a wide range of conditions such as low density of seed nodes, irregular node distributions, and uncontrollable motions of both seeds and nodes. However, the approaches of Bergamo and Mazzini and Hu and Evans each require deployment of a minimum number of anchor nodes in a system.
In the sensor-fusion community, the localization problem has been solved by fusing the observations from different modality of sensors. J. K. Solomon, and Z. H. Lewantowicz, “Estimation of atmospheric and transponder survey errors with a navigation Kalman filter,” In Proceedings of the IEEE Aerospace and Electronics Conference, Dayton, Ohio, 1989, pp 140-147, INUs, range and range-rate sensors are fused to precisely localize the position of an airplane. L. Drolet, F. Michaud, and J. Cote “Adaptable sensor fusion using multiple Kalman filters”, In Proc. Int. Conf. Intelligent Robots and Systems (IROS), Takamatsu, Japan, 2000, pp. 1434-1439, (“Drolet et al.”) another sensor fusion strategy is presented for positioning an underwater remotely operated vehicle by using multiple Kalman Filters. P. M. Lee, B. H. Jeon, S. M. Kim, et al., “An integrated navigation system for autonomous underwater vehicles with two range sonars inertial sensors and Doppler velocity log,” In Proc. Conf IEEE TECHNO-OCEAN '04, 2004, pp: 1586-1593 (“Lee et al.”), an integrated navigation system is introduced for autonomous underwater vehicles with inertial and acoustic sensors. The Lee et al. system was implemented under an Extended Kalman Filter (EKF) framework.
The cited works in sensor-network community rely again on anchor nodes, and structural ambiguities are not avoided. The work in the sensor-fusion communities does not exploit the collaborative effect of networking in the form of drift reduction of individual nodes enabled by range constraints.
Several of the above-mentioned methods for obtaining location estimates are centralized, in which data is collected from distributed nodes by a central node at a central location. Centralization puts a strain on bandwidth and throughput, and may suffer from increased latency times due to the high amount of data being transferred, since all of the measurement data needs to be communicated to one location. Centralized algorithms are also inherently unreliable in the case where the central node is disabled or loses communication with other nodes.
Accordingly, what would be desirable, but has not yet been provided, is a system and method for effectively and automatically fusing location and range measurements to determine the position of distributed nodes in GPS-denied environments while reducing the drift errors associated with inertial measurement units.